Optimal Attitude Estimation and Filtering Without Using Local Coordinates 
Part I: Uncontrolled and Deterministic Attitude Dynamics 
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Abstract — There are several attitude estimation algorithms in 
existence, all of which use local coordinate representations for 
the group of rigid body orientations. All local coordinate repre- 
sentations of the group of orientations have associated problems. 
While minimal coordinate representations exhibit kinematic 
singularities for large rotations, the quaternion representation 
requires satisfaction of an extra constraint. This paper treats 
the attitude estimation and filtering problem as an optimization 
problem, without using any local coordinates for the group 
of rotations. An attitude determination algorithm and attitude 
estimation filters are developed, that minimize the attitude 
and angular velocity estimation errors. For filter propagation, 
the attitude kinematics and deterministic dynamics equations 
(Euler's equations) for a rigid body in an attitude dependent 
potential are used. Vector attitude measurements are used 
for attitude and angular velocity estimation, with or without 
angular velocity measurements. 

I. Introduction 

Attitude estimation is often a prerequisite for control- 
ling aerospace and underwater vehicles, mobile robots, and 
other mechanical systems moving in space. Hence, attitude 
estimation of a rigid body has applications in spacecraft 
and aircraft dynamics, unmanned vehicle dynamics, and 
robot dynamics, including walking robots. While attitude 
sensors and the control tasks for which attitude feedback 
are required may be different in these different applications, 
the fundamental importance of obtaining accurate attitude 
data remains common to all these applications. In this paper, 
a new look at the attitude estimation problem is provided, 
which has two essentially new features: (1) the attitude is 
globally represented without using any local coordinates and 
the nonlinear attitude dynamics equation (Euler's equation) 
for rigid bodies is used, and (2) the filter obtained is not 
a Kalman or extended Kalman filter. A global attitude 
representation has been recently used for partial attitude 
estimation with a linear dynamics model (see [16]). However, 
to the author's knowledge, total attitude estimation using a 
global attitude representation and a full nonlinear attitude 
kinematics and dynamics model (without linearization) has 
not been done before. 

Spacecraft attitude determination and filtering is perhaps 
the oldest application for attitude estimation algorithms, and 
the attitude determination problem for a spacecraft from 
vector measurements was first posed in [21]. A sample of 
the literature in this area can be found in [1], [6], [13], [18], 
[19], [21]. Applications of attitude estimation to unmanned 
vehicles and robots can be found in [2], [16], [17], [20]. 



Algorithms that are typically used for attitude estimation 
in such applications are based upon local coordinate rep- 
resentations of the group of rotations, like quaternions, 
Rodrigues parameters, or Euler angles. As is well known, 
minimal coordinate representations of the rotation group, like 
Euler angles, Rodrigues parameters, and modified Rodrigues 
parameters (see [7]), usually lead to geometric or kinematic 
singularities. Quaternion representation of the attitude matrix 
is commonly used, particularly in spacecraft applications, 
where the quaternion estimation (QUEST) algorithm and 
its several variants have been in use for quite some time 
([1]> [19], [15]). Besides the extra constraint (of unit norm) 
that one needs to impose on the quaternion, the quaternion 
representation for a given rotation depends on the sense of 
rotation used to define the principal angle, and hence can be 
defined in one of two ways. Local coordinate representations 
of the attitude usually lead to use of the extended Kalman 
filter (EKF) as an estimator for attitude and angular velocity. 
It is well known that the EKF has problems with convergence 
and stability in the case of large initial condition errors [5]. 
The attitude determination algorithm presented here does 
not use any local coordinate representation of the attitude, 
and is hence free of the drawbacks associated with such 
local representations. Nonlinear attitude estimation filters for 
a rigid body in an attitude-dependent potential field are 
also developed using this attitude determination algorithm. 
These are optimal nonlinear filters that minimize the attitude 
and angular velocity estimation errors at each measurement 
instant, and are hence free of the stability issues confronting 
extended Kalman filters. 

A brief outline of this paper is given here. In Section UTI 
the attitude determination problem for vector measurements 
with measurement noise is introduced, and a global attitude 
determination algorithm which gives a global minimum 
of the attitude estimation error is presented. This section 
also presents some simulation results that demonstrate the 
applicability of this attitude determination algorithm. Section 
IllTl introduces an attitude dynamics model for a free rigid 
body in a potential field, where the inertia properties of the 
body are assumed to be perfectly known. This deterministic 
dynamics model is used to create a filter that estimates both 
the attitude and the angular velocity. Two cases are consid- 
ered here: the presence of angular velocity measurements, 



and their absence; and the filter algorithms for both cases 
are presented. The paper is concluded in Section II VI with 
a summary of results presented, and a discussion on future 
enhancements to the filter algorithm developed here. 

II. Attitude Determination Problem 

The attitude of a rigid body in space is a representation 
of the orientation of a body-fixed coordinate frame to an 
inertial frame. The principal axes of the body and inertial 
frames are related by a linear transformation given by a 
proper orthogonal matrix, which is usually referred to as 
the rotation or orientation matrix. The rotation matrix may 
be represented by various sets of coordinates, like the Euler 
angles, quaternions, or Rodrigues parameters (see [8], [9]). 
The rotation matrices are proper (determinant=+l) orthogo- 
nal matrices that form a group under matrix multiplication; 
this abstract group is denoted SO(3). Hence, the group 
SO(3) is the compact Lie group of orientation-preserving 
isometries on R 3 , and we represent it using the set of 3 x 3 
proper orthogonal matrices, 

CgR 3x3 , s. t. C T C = I 3 = CC T , det(C*) = l. 

A. Attitude Determination from Vector Measurements 

We now formulate the attitude determination problem from 
vector measurements. Let the direction vectors of a few 
known points in an inertial frame I for M 3 be given by 

e;, i = 1,2, . . . ,n, 

and their corresponding direction vectors in a body-fixed 
frame B (fixed to a rigid body of interest) for R 3 be 

bi, i = l,...,n. 

The inertial and body-fixed direction vectors are related by 
the rotation matrix C which rotates the body frame into the 
inertial frame, such that 

ei = CbiV i£{l,2,...,n}. (1) 

Note that the convention followed for the rotation matrix C 
in is that in [3], [12], while the reverse convention of a 
rotation matrix taking the inertial frame to the body frame is 
used in most of the other literature cited here. The convention 
used here makes it easier to represent the body kinematics 
and dynamics in the body frame and the equations of motion 
are left-invariant, i.e., invariant to left multiplication of C 
by a non-singular matrix. The direction vectors hi when 
measured from the body (e.g., a spacecraft), usually contain 
additive measurement errors and the measured direction 
vectors may not coincide with the actual bi = C ■ e,. Let 
the measured direction vectors be given by 

bi = h + 

where i/j are measurement errors that are usually assumed 
to be Gaussian with zero mean. 

The attitude determination problem consists of finding an 
estimate C of the rotation matrix C such that the errors 

e, - Ch 



are minimized. The least squares attitude estimation problem 
would be to 

n 

Minimize - ^ Wife - Cbi) fe - Cb. t ) 
»=i 

with respect to C subject to C G SO(3), where Wi is a 
known wieght factor (positive) usually taken to correspond to 
the statistical standard deviation of the ith measured vector. 
This problem is also known as Wahba's problem [21]. In this 
work, the weight factors are considered as design parameters. 
We define the 3 x n matrices 

E=[e 1 e 2 ... e n ], B = [b\ b 2 ■■■ b n ]. 

The assumption here and throughout the rest of this paper 
is that both E and B are of rank 3; otherwise the attitude 
determination problem is ill-posed. We introduce the trace 
inner product on the space of real n\ x n 2 matrices, i.e., if 
Ai, A 2 G R" lX ™ 2 , then 

(A U A 2 ) =trace(AjA 2 ). 

The above attitude determination problem can then be re- 
stated as follows: 

Minimize J = -{E - CB, {E - CB)W), C G SO(3), 

(2) 

where the estimate of the rotation matrix (attitude) C is the 
only unknown and W = diag(u^) is the positive diagonal 
weight matrix. We can extemize the cost function Jq by 
taking the first variation with respect to C and setting it to 
zero since C is the only unknown to be determined in this 
problem. The extremal solution to this problem is given by 

6 e Jo = ^(-6CB,(E~dB)W) + ^(E-dB, 

—SCBW) = ((E -CB)WB T ,-6d) 

= -trace(£W(£; - CB) J 5C) = 0, (3) 

where SC is a variation in C G SO(3). Since SC is in 
TgSO(3), the tangent space to SO(3) at C, it has the form 

6C = CU, C/gbo(3). (4) 

Hence, from (|3j and @, we get 

trace(£W(i; - CB) T CU) = BW(E T C - B T ) 

is symmetric <J4> BWE C is symmetric, (5) 

since U is skew-symmetric. The above result can be recast 
into the following form: 

L T C = C T L, L = EWB T , (6) 

and L is known since E is known, B is known from 
measurements, and W is known as a design parameter. 

The following result gives a necessary condition for the 
attitude matrix C that satisfies Q, and is equivalent to 
equation l|6}. 

Lemma 1. Define the linear map Ml : SO(3) — > so(3) by 
M L (C) =C T L- L T C, CgSO(3), (7) 



where L is as defined by (|6|). If C £ SO(3) is in the kernel 
of this map, then C is of the form 



C = SL, S = S 1 , (8) 
i.e., C — SL where S is a 3 x 3 symmetric matrix. 
Proof. If C is in the kernel of Ml, then 

C T L = L T C => LC T = CL T . 

Hence, D — LC is symmetric and we could express L = 
DC, where D — D is symmetric. Now from our earlier 
assumptions, L = EWB is non-singular, since E and B 
are of rank 3, and W £ R nx ™ is positive definite. Thus 
D = LC T is also non-singular. Hence, we get 

DC = L => C = D- 1 L = SL, 

where S = D^ 1 is symmetric. This proves the result. □ 
This result is a special case of Proposition 1 in [4], in 
which C is replaced by a matrix whose row vectors form 
an orthonormal set. However, the above result does not give 
the unique solution to the attitude determination problem (0 
since it does not give an expression for S, from which the 
estimate C of the unknown attitude can be determined. 

To obtain the C that minimizes the cost function Jo, we 
apply the sufficient condition for a minimum by taking its 
second variation with respect to C. The first variation of Jo where 
in can be written as 

SqJq = (L.-8C) = -{L,CU). 

Thus, a sufficient condition for C to minimize the cost 
function Jq is as follows: 



5 2 d J = -trace(i T CC/ 2 ) - trace (£ T CTZ7) 



= -ti-ace(i T CC/ 2 ) > 



This condition, along with Lemma ^ leads to the following 
result. 

Proposition 1. The cost function Jo in (0 is minimized by 
C = SL such that the symmetric matrix S is positive definite. 

Proof. From Lemma ^ we know that a necessary condition 
for the minimizing C would be C = SL, where S is a 
symmetric matrix. Hence 

D = L T C = L T SL 

is symmetric. From condition (|9j, we have trace(Z)[/ 2 ) < 0. 
Since U € so (3), U 2 is symmetric and has negative definite 
trace. Let Q\,Qi S 0(3) be such that 

d = q 1 a 1 qJ, -u 2 = q 2 k 2 qI, 

are the spectral decompositions of D and — U 2 respectively, 
and Ax, A 2 are diagonal. Then 

c = trace(-L>C/ 2 ) = trace^AxQ^AaQj) > 
=> trace(AiQ7<22A 2 QjQi) > fj 

trace(AiPA 2 P T ) > 



where P = Q1Q2 € 0(3). We denote the columns of P by 
Pi, pi, P3 € M 3 and the entries of the diagonal matrix Ai 
as h, l 2 , and 1%. Since U € so(3), the eigenvalues of — U 2 
(the entries of the diagonal matrix A 2 ) are 0, ui > 0, and 
u 2 > 0. Thus, the sufficient condition (|9j is equivalent to 



c = uitrace(Aip 2 P2 ) + W2trace(Aip3p 3 ) > 

=► {hpli + hpl 2 + ht%a) > 

and {hPu + hpj 2 + hph) > 0, 

which is possible for arbitrary p 2 , p% S R 3 if and only if 
li > 0, l 2 > 0, and Z3 > 0. Hence, the sufficient conditon 
(|9} is equivalent to D = QiA\Q\ being positive definite, 



i.e., v Dv > for any v S 



This in turn implies that 



S = {LJ) 1 DL 1 is also positive definite, since vJ Su 
v Dv > where u — Lv. □ 
Lemma ^and Proposition ^ give necessary and sufficient 
conditions, respectively, for the attitude matrix that min- 
imizes Jo- The following result gives an unique attitude 
matrix C € SO (3) that solves the attitude determination 
problem (0 and satisfies equations (|6) and (|9j. 

Theorem 1. The unique minimizing solution to the attitude 
determination problem @ is given by 



C = SL, s = qJ{rr t )- 1 q 



(10) 



L = QR, QeSO(3), (11) 

and R is upper triangular and non-singular (since L = 
EWB^ is non-singular); this is the QR decomposition of 
L. The matrix square root used here is the positive definite 
(principal) square root of a positive definite symmetric 
matrix. 

Proof: From Lemma [T] we know that C = SL = SEWB T 
(9) where S — S , is a necessary condition for the extremal 



solution. From Proposition ^ the equivalent condition to the 
sufficient condition (|5Jl is that the symmetric matrix S has to 
be positive definite. Using the QR decomposition of L given 
by il It . we can express the orthogonality condition of C as 
follows: 

dd T = SQRR T Q T S = I 3 . 
Since S is symmetric, S is given by 



S = y Q(RR T )- 1 Q T = Q\J (RR T )- 1 Q T , 

where the principal (positive definite) square root is taken, 
as given by equation dlOj . This makes S positive definite as 
well. By construction, C = SL satisfies CC^ — I3. Now 
we check the determinant of C = SL, as follows: 

detC = detSdeti 

= (det Q T det \J (RR T y 1 det Q) (det Q det R) 

= / detQ)2 detQdetfl 
Vdeti?deti? T 

' : det Q det R 



^(det R) 2 

- — - det Q det R 
det R 



detQ 



since Q S SO(3). This proves that C G SO(3), and is hence 
the unique minimal solution to the attitude determination 
problem (0. □ 
Although we have used the QR decomposition for the matrix 
L here, one can use the singular value decomposition or any 
other decomposition using orthogonal matrices, to show this 
result. 

We next show that the attitude estimate given by this 
algorithm is unbiased, i.e., in the absence of measurement 
errors, this estimate gives the actual attitude. 

Proposition 2 . The at titude determination algorithm given 
by equations MCft - \ll\ gives an unbiased estimate of the 
attitude. 

Proof: Let us assume that there is no error in the measure- 
ment of body vectors, i.e., B = B = [b\ b^ ... b n ]. In 
that case, we have E = CB, and 

L = L = EWB T = EWE T C 
=> C = (EWE T )- 1 L , 

which, by Theorem ^ implies that 



Simulation Data: 



S n = Q^(R RI)- 1 qI = (EWE 1 ) 
where Lq = QqRo- This is equivalent to 



T\-i 



EWE 
(EWE 7 ) 2 



) 



But the right-hand side above is LqL 7 
(EWE T C)(C T EWE T ), which is equal to the left-hand 
side. Thus, we have C = S L where S = (EWE 1 )- 1 



as 



required. This proves that this algorithm is unbiased. □ 
These results are used as the basis for attitude estimation 
filters obtained in Section ITTT1 



B. Simulation results for Attitude Determination Algorithm 

We end this section with a simulated example of an 
attitude determination problem, where an attitude matrix is 
obtained from 'measurements' of seven (unit) vectors, repre- 
senting seven different directions in Euclidean 3-space. The 
"measured" vectors are given as normalized (unit) vectors. 
The simulation is carried out using a MATLAB program, 
which implements the attitude determination algorithm given 
in Theorem^ The simulated vectors "measured" in the body 
frame have added Gaussian noise with a standard deviation of 
a = 0.002 rads w 0.115° (which is relatively large compared 
to the capabilities of most modern attitude sensors). 

The data used in this simulation, in terms of the inertial 
unit vectors, and their "measured" counterparts in the body 
frame, are as follows: 



E = 



B 



0.3817 0.3077 
-0.5450 -0.6045 
0.7465 0.7347 

0.3161 
-0.6582 - 
0.6832 

0.1287 0.0975 
-0.9628 -0.9843 
-0.2394 -0.1517 

0.0210 
-0.9904 
-0.1414 



0.2324 
-0.5824 
0.7789 

0.2975 

-0.6046 

0.7389 

0.1580 
-0.9833 
-0.0862 

0.1020 

-0.9829 

-0.1404 



0.3374 
-0.5675 
0.7511 

0.2807 

-0.5912 

0.7561 

0.1264 
-0.9750 
-0.1904 

0.1249 
-0.9836 
-0.1279 



Note that the vectors in the inertial frame are clustered 
together, and hence, so are the vectors in the body frame. 
This simulates direction vectors as would be measured by 
an optical instrument with a finite field of view, e.g., a star 
tracker. The "actual" attitude matrix which takes the "actual" 
body directions to the inertial directions, is assumed to be 
known for this simulation, and is given by 



C = 



-0.2029 -0.1865 -0.9613 
0.6385 0.7191 -0.2743 
0.7424 -0.6694 -0.0269 



(12) 



As mentioned before, these simulated measurements of body 
directions correspond to added Gaussian noise of 0.002 
radian to the "actual" body directions. 

The results of this simulation, in the form of the attitude 
matrix determined by this algorithm C, the error between the 
known "actual" attitude matrix C and the attitude C, and the 
error e = E — CB, are given below. 
Simulation Results: 



C = 



ec = C L C-h = 



E-CB = 



-0.0009 
-0.0007 
-0.0007 



-0.2042 
0.6386 
0.7420 

-0.0000 
-0.0006 
-0.0012 

0.0008 
0.0008 
0.0012 

0.0007 
0.0003 
0.0016 



-0.1856 
0.7190 
-0.6698 

0.0006 
-0.0000 
0.0008 

-0.0006 
-0.0000 
0.0006 

0.0009 
0.0009 
-0.0016 



-0.9612 
-0.2745 
-0.0283 

0.0012 
-0.0008 
-0.0000 

-0.0007 
0.0005 
-0.0012 

0.0008 
0.0009 
0.0011 



Note that the error in the attitude matrix is here specified 
as C L C minus the identity matrix; one can also specify this 
error as C — C. As defined here, the matrix 1$ + ec = 
C^C € SO(3), is the measure (in the group of rigid body 
rotations) of the attitude error. The maximum errors in these 
results are of the order of the measurement errors in the body 
vectors, which demonstrates the applicability of this attitude 
determination algorithm. 



III. Attitude Estimation Filters for a Free Rigid 
Body in a Potential Field 

In this section, we develop an attitude estimation filter 
based on the attitude determination algorithm of Theorem ^ 
developed in the last section. We assume that the attitude 
dynamics is perfectly known, and is that of a free rigid 
body in a potential field, i.e., there are no applied (control) 
forces on the body. We leave the potential field to be general 
(could be uniform or central gravity, for example). Two 
cases are dealt with here: (1) the case without, and (2) 
the case with angular velocity measurements. Since we use 
the actual (continuous) nonlinear dynamics equations for 
filter propagation, the filters developed here are not extended 
Kalman filters; they are nonlinear filters. We also assume 
that the vector attitude measurements (and angular velocity 
measurements, if any), are made at discrete time instants. 
Hence, the filter equations obtained are of the continuous- 
discrete type. 

Let {tk}, k £ Z non-negative, denote an increasing 
sequence of non-negative real numbers that coincide with 
time instants at which measurements of vectors in the body 
frame (and angular velocity measurments, if any) are taken. 
Let Bk £ R 3xrlfc denote the set of rik vector measurments 
taken at time tk in the body frame; the columns of Bk denote 
the measured body vectors. If Ek £ E 3x " fc denotes the 
vectors in the inertial frame, and Ck is the actual attitude 
matrix from the body to the inertial frame, then 

Bk = clE k + Nk, (13) 

where the columns of Nk £ R 3x " fc are the measurement 
errors in the body vectors. The measured body vectors 
are usually expressed as unit vectors. The angular velocity 
measurement at time tk is denoted by £ so (3). At 
time tk, k > 1, the attitude and angular velocity estimates 
obtained by propagating using the attitude kinematics and 
dynamics equations from time tk-i are denoted C7 and 
fi^T respectively, and the updated attitude at this time instant 
obtained from the attitude determination part of the filter 
(which is based on the algorithm of Section |ll|l is denoted 
Ck ■ The angular velocity is also updated at the measurment 
time tk, and the updated angular velocity estimate is denoted 

tot 

A. Dynamics of Free Rigid Body in a Potential Field 

We first obtain the dynamics (equations of motion) of a 
free rigid body in a potential field in a compact geometric 
form, that is free of any particular coordinate description. 
The attitude kinematics is given by 

c = cn, (14) 

where fl £ so (3) denotes the angular velocity in the body 
frame. Let A denote the symmetric positive definite inertia 
matrix of the rigid body. The Lagrangian for the rigid body 
in a potential field is given by 

c(c,n) = ^(si,nA)-v(c), (15) 



where the first term is the kinetic energy, and V(C) denotes 
the potential energy that is dependent on the attitude of the 
body. 

The equations of motion are obtained by applying Hamil- 
ton's principle to the action quantity 

S= [ £(C,{l)dt, 
Jo 

and taking reduced variations on the group SO(3) (see [3], 
[12]). The reduced variations at the point (C, 0) £ TSO(3) 
are given by ([3], [12]) 

<5C* = C£, 5n = £ + [fi,E], (16) 

where £(t) £ so(3) specifies a variation vector field on 
SO (3) that vanishes at the end-points, i.e., E(0) = E(T) = 
0. Extremizing the action along this vector field, we get 

5S = Jo fy dn > nA ) + \( n > 6nA ) -<Vb,£)jdt 
= J l~{J(n),8Sl)-(dcV,C2)}=0, 

where dcV — ^ and J : so(3) — > so(3) is a positive 
definite operator on the Lie algbera so (3) that is defined by 

= An + riA. (17) 

The second term arising from the potential in the first 
variation above, can be rendered as 

(c T d c v^) = l -{c 7 d c v- (d c v) T c,j:). 

Using the reduced variations as given by d!6i and the 
relations (see [14]) 

(-7(40, A a ) = (Ax, J(A 2 )}, ([X,Y],Z) = (X,[Y,Z]), 

we get (integrating by parts) 

2SS = {(J(0),S+[0,E]) 

-(C T d c V- (a c ^) T c,s>}dt 
-<j(n),s)}dt + (j(n),E)|J' = o. 

Since we take arbitrary fixed end-point variations, the last 
term above vanishes, and the terms in the integral give us 
the dynamics 

j(n) = n] - c T d c v + {dcvfc. (is) 

This dynamics equation is also derived in [11] in a similar 
fashion. 

We now present the result that any given value of J(O) for 
a given symmetric positive definite A, uniquely determines 
the skew-symmetric matrix fL 

Lemma 2. If K is symmetric and positive definite and X £ 
So(n), the map Jk '■ so(n) — > so(n) given by Jk '■ X 
KX + XK has kernel zero, and is hence an isomorphism. 



Proof: Since X is skew, there exists a unitary matrix L, i.e., 
LL T = Z T L = I N , such that iXL T = iS, where £ is a 
real diagonal matrix (X is unitarily diagonalizable). Thus if 
KX + XK = 0, then 

lkl j j: + ZLKL T = 0, 

— 'I - 

where LKL is a positive definite Hermitian matrix. If e is 
an eigenvector of K = LKL? , then 

= Ae, A > 0, 

and 

+ Y.K = =>■ K(Ee) + A(Se) = 0. 

Hence Se is also an eigenvector of if with eigenvalue 
— A < 0. But if is positive definite and so all its eigenvalues 
are strictly positive. Thus, we have a contradiction, unless 
£ = and hence X = 0. □ 
This lemma and its proof are also given in [4]. Note that 
we only need this result for so(3) here, although we have 
stated and proved it for all so(n). Thus, if the momentum 
M = J (ft) is given, then one can obtain the unique 
angular velocity corresponding to it, il = J _1 (M) G So(3). 
From equation (II 81 and Lemma |2] this uniquely determines 
il given the values of il and C at any instant. We use 
this dynamics equation, along with the attitude kinematics 
equation dl4> . to propagate the attitude and angular velocity 
between discrete sets of measurements. 

B. Attitude Estimation Filter without Angular Velocity Mea- 
surements 

We use the attitude determination algorithm given in 
Section|ll]to form an attitude estimation filter, by augmenting 
angular velocity data. The algorithm presented here works 
when there are body vector measurements at discrete time 
instants, but no available angular velocity measurements. 
However, we assume that we know the initial angular veloc- 
ity. We use equations (I14> and ( I18> to integrate the attitude 
and angular velocity in time between the sets of body vector 
measurements; this corresponds to the propagation phase of 
the filter. The attitude and angular velocity are then updated 
based on the body vector measurements, in a manner similar 
to that used in a Kalman filter. 

We obtain the filter as an optimal filter from a suitable 
cost function that minimizes errors between the estimated 
and the measured attitude, as well as the difference between 
the propagated and updated estimates. The update of the 
attitude estimate C k at measurement instant t k is obtained 
by minimizing the following cost function with respect to 



J„ = -^{(E fc -ft B k ,(E k -Ck B k )W k ) 

k=0 

+ ((C k ~) T C k + -/,((CT) T Cfe + -J)A», (19) 

where A is a symmetric positive definite matrix that can 
be chosen as a design parameter for the filter. In the case 
that we are considering now, there are no angular velocity 



measurements; therefore we update the angular velocity by 
minimizing the difference between the rates of change of 
attitude at a measurement instant: 



Jr = -^{Ck Hk — Ck ilk ~ C k ilk )n), 

(20) 

where II is a symmetric positive definite matrix that can 
be chosen as a design parameter for the filter. Here Ck 
and ilk are obtained by integrating equations H41 and 
Jl 8i respectively, from time tk-i to time tk, with initial 
conditions Ck-x and ilk—i respectively. 
The first variation of Ck is given by 

SCk + = Ck + U+, U+eso{3). 
Setting the first variation of J a in JT9l to zero, we get: 

SJ a = ^2 {({Eh - Ck + B k )W k Bk , -Ck + U+) 

k=0 

+ ((Ck + ) T Ck~A((Ck~) T Ck + -I),U+)} 

N + - ~T 

= ) T ( Cfe A + E k WkBk ),U+)} = 0, (21) 

fe=i 

taking into account the initial condition Co = Cq , which 
is either assumed to be known from a given initial attitude, 
or obtained from an initial set of measurements Bq using the 
algorithm of Theorem ^ From the expression J21l i. we get 
the result 

— + T - ~T 

(Ck ) Lk is symmetric, where Lk = Ck A + EkWkBk ■ 

(22) 

Now the result of Theorem ^ can be applied to obtain 
the update of the attitude estimate Ck in terms of the 
QR decomposition of Lk- Given this update of the attitude 
estimate, one can obtain an update of the angular velocity 
estimate by minimizing J r in d20> with respect to ilk ■ This 
gives us: 

sj r = ((dl + n k + -cl~ih~)Ti,Ck + 5iik + ) = o 

=> (ilk — (Ck )^Ck il k )n is symmetric. 

The initial angular velocity ilo is assumed to be known, 

and from J26i . Hq = ilo ■ The above analysis can be 
formalized into the following result, which is one of the main 
results of this paper. 

Theorem 2. The attitude estimation filter obtained from 
minimizing the cost functions J a and J r is given by the 
attitude and angular velocity updates: 

6k + = s k L k , ^ + n + n^ + = (Ck + ) T Ck~n k ~n 

+nn k ~(C k ~) T C k + , (23) 

where 

QkRk = L k = Cl A + E k W k B k , (24) 



is the QR decomposition of L k , and 



S k = Qk\/ {RkRl^Ql (25) 



is symmetric. The initial conditions for the filter are 
Co = d j ^o = ^o , 



(26) 



where Co is either given or obtained from an initial set 

of measurements Bq, and f2o ' s given. The propagation 
equations for the filter are given by: 



C k 



k+i 



(Cfi)dt, 



(27) 



n 



= r 



[J(Q),n]-V c (C))dtj{28) 



where C(t k ) = Cfc and £l(t k ) = fife . 

Note that according to Lemma [2] equations (I23> and (12 8 1 
uniquely determine fl k and Qfc+i respectively. The result 
of Proposition |2] also holds, i.e., the attitude estimate given 
by this result is unbiased. This can be shown in a similar 
manner to the proof of Proposition |2] If IT = I (the identity 
matrix), then equation d23t for the update of the angular 
velocity estimate simplifies to 



1 



=o (c fc yc k n k + n k (c k yc k 



(29) 



This equation can be readily used for updating angular 
velocity estimates without angular velocity measurements 
in the filter implementation. For the propagation equations 
(127 \ and (12 8 1 may be implemented by numerical integration 
software, including variational integrators (see [10], [11]) 
that preserve the group structure of SO(3). 

C. Attitude Estimation Filter using Angular Velocity Mea- 
surements 

The creation of an attitude estimation filter from the basic 
attitude determination algorithm in Section Ql] is made easier 
when angular velocity measurements are available. In this 
case, we assume that the sampling instants for attitude and 
angular velocity measurements are the same. The body vector 
(attitude) measurements are given by Jl 31 . while the angular 
velocity measurements are given by 



o fe = n k + P k , 



(30) 



where Q k = tl(t k ) is the actual angular velocity and P k is a 
zero mean measurement error from a stochastic process with 
known statistics. Extensions can also be made to deal with 
the case when the sampling instants for attitude and angular 
velocity measurements are different. 

The optimal filter when attitude and angular measurements 
are available is obtained by minimizing the following cost 
function with respect to C k and £l k : 



1 N + ~ + ~ 

Jb = Ja + ^<^ + - - 



(31) 



where J a is as defined in (I19> . and A and T are symmetric 
positive definite matrices that can be chosen as design 
parameters for the filter. The matrix X k is also a symmetric 
positive definite matrix, which can be assigned as the error 
covariance matrix for the angular velocity measurement error 
P k in OS. 

We take reduced variations on SO (3) with the first vari- 
ations of Cfc and fl k given by: 

scl + = c~ k + u+, sn k + = u+ + [n k + , u+], 

where Ujt G so (3). The necessary condition for optimality 
is given by equating the first variation of J\> with respect to 
C k and Qfc to zero. This gives us: 

N 

= ]T {<[G fe , ^ + ]-(C;. + ) T L fe , U+) + {G k , {/+)} = 0, 



fc=0 



where 



G k 



(J?* -n k )x k + (n k -n k )r, 



L k = E k W k B k +C k A. 



(32) 



(33) 



Since U~£ and U£ are independent of each other, the second 
term in equation d32l > implies that G k is symmetric. This 
in turn implies that [Gk , Oft ] is also symmetric, and hence 
([Gfc,fifc ],Uu) — 0. Thus, the first term in equation d32l > 

implies that (C' k )^L k is symmetric. Now we can apply 
Theorem \l\ to obtain the update of the attitude estimate. 
The attitide and angular velocity updates are given in the 
following result. 

Theorem 3. The attitude estimation filter o btai ned from 
minimizing the cost function J a in equation is given 
by the attitude and angular velocity updates: 

Cfc + = s k L k , j Xk+r {n k + ) = j Xk (n k )+j r (n k ~), (34) 

where Jk ■ So(n) — > So(n) for a symmetric positive definite 
matrix K is as defined in Lemma 



Q k R k = L k = C k A + E k W k B k 
is the QR decomposition of L k , and 



(35) 



S k = Q k \/(R k Rl)- 1 Ql (36) 



is symmetric. The initial conditions for the filter are 

-+ 



Co 



Co , fio 



n + , 



(37) 



fc=0 



where Co andQ§ are jeither given or obtained from initial 
measurements Bq, and D,q. 

The propagation equations for the filter are equations 

d27Vll2"8l where C(t k ) = Ck + and Sl(t k ) = £h + ■ Thus, 
the propagation phases for the filters given by theorems [2] 
and |3] are identical. Note that, by Lemma [2] equation J34l > 
determines n k uniquely since X k + T is positive definite. 
Also, the result of Proposition [2] holds for the attitude 
estimate given by this filter. The angular velocity estimate 



is also unbiased, since in the absence of angular velocity 
measurement errors, = fij. = Oj., and equation J34l > 
gives £2fc = f2fc. The filters developed in this section can 
also be extended to estimate a constant bias in measurements, 
if sensor bias is present. 

IV. Conclusions 

This paper presents an attitude determination algorithm 
and attitude estimation filters that can be used for attitude 
estimation of robots, spacecraft, and other vehicles. The 
attitude determination algorithm is obtained from an opti- 
mization process with the cost function equal to a weighted 
attitude estimation error on the group of rigid-body orienta- 
tions. This algorithm is global, and does not use any local 
coordinate representation (like Euler angles or quaternions) 
for the group of orientations. The optimization is carried 
out with variations on the smooth manifold (Lie group) of 
rigid body orientations, and the estimate obtained is shown 
to (globally) minimize the attitude estimation error. It is 
also shown to provide an unbiased estimate of the attitude, 
i.e., in the absence of measurement errors, the estimate 
of the attitude obtained is the actual attitude. A numerical 
simulation of this attitude determination algorithm, with a 
set of seven simulated body unit vector measurements with 
noise for seven given inertial unit vectors, is carried out. 
The order of error in the attitude estimate obtained using 
this algorithm is found to be no more than the order of the 
error in measurements. 

The attitude estimation filters are of the continuous- 
discrete type, which work with a continuous deterministic 
dynamics model supplemented by discrete sets of noisy 
measurements. These filters are obtained for two cases: when 
there are body vector measurements but no angular velocity 
measurements, and when there are attitude and angular 
velocity measurements at the same measurement instants. 
It is assumed that the attitude dynamics is deterministic, 
and accurately known. The (nonlinear) attitude kinematics 
and dynamics equations are used for propagation of the 
attitude and angular velocity between successive sets of 
measurements. The attitude and angular velocity estimates 
are obtained from an optimization process that minimizes 
the weighted sum of errors between the estimates and mea- 
surments, and between the estimates and propagated values 
for these quantities at each measurment instant. These filters 
are shown to be unbiased, i.e., in the absence of measurement 
errors, the estimates they give are equal to the actual attitude 
and angular velocity. 

This work is a preliminary exploration into attitude 
estimation techniques without using any local coordinate 
representation for the attitude. The results obtained thus 
far are encouraging, and they show that one can obtain 
unbiased filters that minimize the errors in attitude and 
angular velocity estimation without using local coordinate 
representations and without using an extended Kalman fil- 
ter. While local coordinate representations of attitude have 
problems associated with singularities in attitude kinematics 
or additional constraints, the extended Kalman filter has 



problems associated with convergence of estimates for large 
initialization errors. These drawbacks are not present in the 
filters developed here, since they do not use local coordinates, 
and since they give optimal nonlinear filters that minimize 
the attitude and angular velocity estimation errors at each 
measurement instant. Thus, the attitude estimation filter 
algorithms developed here fill a gap in the existing research 
in this direction, besides improving upon the filters currently 
in use for attitude estimation of mechanical systems. 

Future work would include extension of the attitude and 
angular velocity estimation filters developed here to the case 
when the dynamics has modeling errors or noise. Numerical 
and/or experimental studies in implementation of these filters 
could also be explored. Numerical simulation results for 
the filters developed here, and numerical comparisons with 
estimation algorithms using local coordinates and extended 
Kalman filters for the deterministic dynamics case, have not 
been obtained yet. Such results are very likely to be reported 
in the near future. 
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